#include "hdf_device_desc.h"
#include "hdf_log.h"
#include "gpio_if.h"
#include "osal_io.h"
#include "osal_thread.h"
#include "osal_spinlock.h"
#include "osal_time.h"
 
#define HDF_LOG_TAG ptz_driver

#define GPIO_ID(GROUPID,PINID) ((GROUPID)*10 + (PINID))

const uint16_t g_hMotorGpioIDs[] = {GPIO_ID(6,4),GPIO_ID(6,5),GPIO_ID(6,6),GPIO_ID(6,7)};

const uint16_t g_vMotorGpioIDs[] = {GPIO_ID(1,4),GPIO_ID(1,5),GPIO_ID(1,6),GPIO_ID(1,7)};

const uint8_t rotateTable[] = {0x10,0x30,0x20,0x60,0x40,0xc0,0x80,0x90};

#define ACT_VMOTOR(VALUE) OSAL_WRITEB(VALUE, IO_DEVICE_ADDR(0x120B6000 + 0x3c0))
#define ACT_HMOTOR(VALUE) OSAL_WRITEB(VALUE, IO_DEVICE_ADDR(0x120B1000 + 0x3c0))

#define VMOTOR_PIN() OSAL_READB(IO_DEVICE_ADDR(0x120B6000))
#define HMOTOR_PIN() OSAL_READB(IO_DEVICE_ADDR(0x120B1000))

static inline int16_t MotorDegressToSteps(int16_t degress) {
    return (int16_t)(2 * (degress * 16)/5.625);
}


OSAL_DECLARE_THREAD(g_motorThread);
OSAL_DECLARE_SPINLOCK(g_motorLock);

struct MotorTaskPara{
    uint8_t currHStep;
    uint8_t currVStep;
    int32_t rotateHSteps; 
    int32_t rotateVSteps;
} ;

struct MotorTaskPara g_MotorTaskPara  = { 
    .currHStep = 0,
    .currVStep = 0,
    .rotateHSteps = 0,
    .rotateVSteps = 0
};

enum MotorID {
    H_MOTOR = 0,
    V_MOTOR
};

static bool g_taskRunning = false;
uint32_t g_stepDelay = 800;

int MotorThread(void *para){
    g_taskRunning = true;
    do {
        (void)OsalSpinLock(&g_motorLock);
        if(g_MotorTaskPara.rotateHSteps > 0) {
            --g_MotorTaskPara.rotateHSteps;
            ACT_HMOTOR(rotateTable[g_MotorTaskPara.currHStep]);
            --g_MotorTaskPara.currHStep;
            g_MotorTaskPara.currHStep %= sizeof(rotateTable);            
        } else if(g_MotorTaskPara.rotateHSteps < 0) {
            ++g_MotorTaskPara.rotateHSteps;
            ACT_HMOTOR(rotateTable[g_MotorTaskPara.currHStep]);
            ++g_MotorTaskPara.currHStep;
            g_MotorTaskPara.currHStep %= sizeof(rotateTable); 
        } else {
            ACT_HMOTOR(0);
        }

        if(g_MotorTaskPara.rotateVSteps > 0) {
            --g_MotorTaskPara.rotateVSteps;
            ACT_VMOTOR(rotateTable[g_MotorTaskPara.currVStep]);
            --g_MotorTaskPara.currVStep;
            g_MotorTaskPara.currVStep %= sizeof(rotateTable);            
        } else if(g_MotorTaskPara.rotateVSteps < 0) {
            ++g_MotorTaskPara.rotateVSteps;
            ACT_VMOTOR(rotateTable[g_MotorTaskPara.currVStep]);
            ++g_MotorTaskPara.currVStep;
            g_MotorTaskPara.currVStep %= sizeof(rotateTable);
        } else {
            ACT_VMOTOR(0);
        }
        (void)OsalSpinUnlock(&g_motorLock);
        HDF_LOGD("%s:HStep=%d,Hpin=%x,VSTEP=%d,Vpin=%x", __func__, g_MotorTaskPara.currHStep, HMOTOR_PIN(), g_MotorTaskPara.currVStep, VMOTOR_PIN());
        OsalUDelay(g_stepDelay);
    }while(g_MotorTaskPara.rotateHSteps != 0 || g_MotorTaskPara.rotateVSteps != 0);
    HDF_LOGW("%s:thread exit!", __func__);
    g_taskRunning = false;
    ACT_HMOTOR(0);
    ACT_VMOTOR(0);
    return HDF_SUCCESS;
}

void AddRotate(enum MotorID motor, int16_t rotate) {
    (void)OsalSpinLock(&g_motorLock);
    if(motor == H_MOTOR) {
        g_MotorTaskPara.rotateHSteps += MotorDegressToSteps(rotate);
    } else if(motor == V_MOTOR) {
        g_MotorTaskPara.rotateVSteps += MotorDegressToSteps(rotate);
    }else {
        HDF_LOGE("No such motor.motorID=%d", motor);
    }
    if((g_MotorTaskPara.rotateVSteps != 0 || g_MotorTaskPara.rotateHSteps != 0) && !g_taskRunning) {
        struct OsalThreadParam para = {
            .name = "MotorThread",
            .priority = OSAL_THREAD_PRI_HIGH,
            .stackSize = 0x2000
        };
        (void)OsalThreadStart(&g_motorThread, &para);
    }
    (void)OsalSpinUnlock(&g_motorLock);
}

#define CHECK_REG32(ADDR,VALUE) do { \
        uint32_t regValue = OSAL_READL(IO_DEVICE_ADDR(ADDR)); \
        if(regValue == (VALUE)) { \
            HDF_LOGE("%s:REG(%x) is as EXPECTED!", __func__, (ADDR)); \
        } else {\
        HDF_LOGE("%s:REG(%x) current is %x", __func__, (ADDR), regValue); \
        } \
    }while(false)

#define CHECK_REG8(ADDR,VALUE) do { \
        uint8_t regValue = OSAL_READB(IO_DEVICE_ADDR(ADDR)); \
        if(regValue == (VALUE)) { \
            HDF_LOGE("%s:REG(%x) is as EXPECTED!", __func__, (ADDR)); \
        } else {\
        HDF_LOGE("%s:REG(%x) current is %x", __func__, (ADDR), regValue); \
        } \
    }while(false)

int32_t PTZMessageDispatch(struct HdfDeviceIoClient *client, int cmdId, struct HdfSBuf *reqData, struct HdfSBuf *reply) {
    if(reqData == NULL) {
        HDF_LOGE("%s: reqData is NULL!", __func__);
        return HDF_FAILURE;
    }
    if(cmdId == 0) {
        uint8_t motorID = H_MOTOR;
        int16_t roa = 0;
        bool ret = HdfSbufReadUint8(reqData, &motorID);
        ret = ret && HdfSbufReadInt16(reqData, &roa);
        if(!ret) {
            HDF_LOGE("%s:read value failed!", __func__);
            return HDF_FAILURE;
        }

        if(motorID > V_MOTOR) {
            HDF_LOGE("%s:bad motor ID!id=%d", __func__, motorID);
            return HDF_FAILURE;
        }

        if(roa > 360 || roa < -360) {
            HDF_LOGE("%s:bad roa found!roa =%d", __func__, roa);
            return HDF_FAILURE;
        }

        AddRotate((enum MotorID)motorID, roa);

        #if 0

        HDF_LOGE("Checking REGs...");
        //pin12
    CHECK_REG32(0x112C0074, 0X1200);
    //pin13
    CHECK_REG32(0x112C0070, 0X1200);
    //pin14
    CHECK_REG32(0x112C006C, 0X1A00);
    //pin15
    CHECK_REG32(0x112C0068, 0X1200);

    //pin80
    CHECK_REG32(0x120C0010, 0X1A02);
    //pin81
    CHECK_REG32(0x120C0014, 0X1A02);
    //pin82
    CHECK_REG32(0x120C0018, 0X1A02);
    //pin85
    CHECK_REG32(0x120C001C, 0X1202);

    //GPIO6
    HDF_LOGE("checking GPIO6...");
    CHECK_REG8(0x120B6000 + 0x3c0, 0xf0);
    CHECK_REG8(0x120B6000 + 0x400, 0xf0);

    HDF_LOGE("Write using regs...");
    OSAL_WRITEB(OSAL_READB(IO_DEVICE_ADDR(0x120B6000 + 0x400)) | 0xf0, IO_DEVICE_ADDR(0x120B6000 + 0x400));
    OSAL_WRITEB((uint8_t)0xf0, IO_DEVICE_ADDR(0x120B6000 + 0x3c0));

    HDF_LOGE("check again...");
    CHECK_REG8(0x120B6000 + 0x3c0, 0xf0);
    CHECK_REG8(0x120B6000 + 0x400, 0xf0);


    HDF_LOGE("checking GPIO1...");
    CHECK_REG8(0x120B1000 + 0x3c0, 0xf0);
    CHECK_REG8(0x120B1000 + 0x400, 0xf0);

    HDF_LOGE("Write using regs...");
    OSAL_WRITEB(OSAL_READB(IO_DEVICE_ADDR(0x120B1000 + 0x400)) | 0xf0, IO_DEVICE_ADDR(0x120B1000 + 0x400));
    OSAL_WRITEB((uint8_t)0xf0, IO_DEVICE_ADDR(0x120B1000 + 0x3c0));

    HDF_LOGE("check GPIO1 again...");
    CHECK_REG8(0x120B1000 + 0x3c0, 0xf0);
    CHECK_REG8(0x120B1000 + 0x400, 0xf0);

    #endif

    } else if(cmdId == 1) {
        uint8_t motorID = H_MOTOR;
        int16_t value = 0;
        bool ret = HdfSbufReadUint8(reqData, &motorID);
        ret = ret && HdfSbufReadInt16(reqData, &value);
        if(!ret) {
            HDF_LOGE("%s:read value failed!", __func__);
            return HDF_FAILURE;
        }

        if(motorID > V_MOTOR) {
            HDF_LOGE("%s:bad motor ID!id=%d", __func__, motorID);
            return HDF_FAILURE;
        }

        if(value < 50) {
            HDF_LOGE("%s:step delay must greater than 50", __func__);
            return HDF_FAILURE;
        }

        g_stepDelay = (uint32_t)value;
    }
    return 0;
}

int32_t PTZOpen(struct HdfDeviceIoClient *client) {
    HDF_LOGW("%s in...");
    return 0;
}

void PTZServiceRelease(struct HdfDeviceIoClient *client){

}
 
//驱动对外提供的服务能力，将相关的服务接口绑定到HDF框架
int32_t HdfPTZDriverBind(struct HdfDeviceObject *deviceObject)
{
    HDF_LOGE("PTZ driver binding...");
    static struct IDeviceIoService ioService = {
        .Open = PTZOpen,
        .Dispatch = PTZMessageDispatch,
        .Release = PTZServiceRelease
    };
    deviceObject->service = &ioService;
    return 0;
}

#define WRITE_32BIT_REG(REG,VALUE) do { \
    HDF_LOGE("%s:change reg 0x%x from %x to %x", __func__, REG, OSAL_READL(IO_DEVICE_ADDR(REG)), VALUE); \
    OSAL_WRITEL(VALUE, IO_DEVICE_ADDR(REG)); \
}while(false)
 
// 驱动自身业务初始的接口
int32_t HdfPTZDriverInit(struct HdfDeviceObject *deviceObject)
{
    HDF_LOGE("PTZ driver Init...");
    //设置个引脚，设置为IO，下拉电阻使能。
    //pin12
    WRITE_32BIT_REG(0x112C0074, 0X1200);
    //pin13
    WRITE_32BIT_REG(0x112C0070, 0X1200);
    //pin14
    WRITE_32BIT_REG(0x112C006C, 0X1A00);
    //pin15
    WRITE_32BIT_REG(0x112C0068, 0X1200);

    //pin80
    WRITE_32BIT_REG(0x120C0010, 0X1A02);
    //pin81
    WRITE_32BIT_REG(0x120C0014, 0X1A02);
    //pin82
    WRITE_32BIT_REG(0x120C0018, 0X1A02);
    //pin85
    WRITE_32BIT_REG(0x120C001C, 0X1202);
    
    int32_t ret;
    for(uint8_t i = 0; i < sizeof(g_hMotorGpioIDs)/sizeof(uint16_t);i++) {
        HDF_LOGE("%s:Contig gpio %d...", __func__, g_hMotorGpioIDs[i]);
        ret = GpioSetDir(g_hMotorGpioIDs[i], GPIO_DIR_OUT);
        if(ret != HDF_SUCCESS) {
            HDF_LOGE("%s:set gpio dir failed!gpioID=%d", __func__, g_hMotorGpioIDs[i]);
        }
        ret = GpioWrite(g_hMotorGpioIDs[i], GPIO_VAL_LOW);
        if(ret != HDF_SUCCESS) {
            HDF_LOGE("%s:write gpio failed!gpioID=%d", __func__, g_hMotorGpioIDs[i]);
        }
    }

    for(uint8_t i = 0; i < sizeof(g_vMotorGpioIDs)/sizeof(uint16_t);i++) {
        HDF_LOGE("%s:Contig gpio %d...", __func__, g_vMotorGpioIDs[i]);
        ret = GpioSetDir(g_vMotorGpioIDs[i], GPIO_DIR_OUT);
        if(ret != HDF_SUCCESS) {
            HDF_LOGE("%s:set gpio dir failed!gpioID=%d", __func__, g_hMotorGpioIDs[i]);
        }
        ret = GpioWrite(g_vMotorGpioIDs[i], GPIO_VAL_LOW);
        if(ret != HDF_SUCCESS) {
            HDF_LOGE("%s:write gpio failed!gpioID=%d", __func__, g_hMotorGpioIDs[i]);
        }
    }

    ret = OsalThreadCreate(&g_motorThread, MotorThread, NULL);
    if(ret != HDF_SUCCESS) {
        HDF_LOGE("%s:Create thread failed!ret=%d", __func__, ret);
        return ret;
    }

    ret = OsalSpinInit(&g_motorLock);
    if(ret != HDF_SUCCESS) {
        HDF_LOGE("%s:Init spinlock failed!ret=%d", __func__, ret);
        return ret;
    }
    return ret;
}
 
// 驱动资源释放的接口
void HdfPTZDriverRelease(struct HdfDeviceObject *deviceObject)
{
    HDF_LOGE("PTZ driver release...");
    return;
}

// 定义驱动入口的对象，必须为HdfDriverEntry（在hdf_device_desc.h中定义）类型的全局变量
struct HdfDriverEntry g_PTZDriverEntry = {
    .moduleVersion = 1,
    .moduleName = "ptz_driver",
    .Bind = HdfPTZDriverBind,
    .Init = HdfPTZDriverInit,
    .Release = HdfPTZDriverRelease,
};
 
// 调用HDF_INIT将驱动入口注册到HDF框架中，在加载驱动时HDF框架会先调用Bind函数,再调用Init函数加载该驱动，当Init调用异常时，HDF框架会调用Release释放驱动资源并退出。
HDF_INIT(g_PTZDriverEntry);